from sympy import *
import csv
import numpy as np
from oneOrthoAuxData import OrthoAuxData

def indirect_position(dem_resampled_path, meta_file_path):
    # 构建卫星轨道模型
    time_array, meta_data = OrthoAuxData.read_meta(meta_file_path)
    orbital_paras, T0 = OrthoAuxData.orbit_fitting(time_array, meta_data)

    # 获取一些参数
    t0 = OrthoAuxData.get_t0(meta_file_path)
    PRF = OrthoAuxData.get_PRF(meta_file_path)
    delta_t = 1 / PRF # 
    R0 = OrthoAuxData.get_R0(meta_file_path)
    delta_R = OrthoAuxData.get_delta_R(meta_file_path)

    λ = OrthoAuxData.get_lamda(meta_file_path)
    LightSpeed = 300000000
    Td, doppler_d = OrthoAuxData.get_doppler_rate_coef(meta_file_path)

    pstn_arr = OrthoAuxData.read_dem(dem_resampled_path, flag=1)
    print(pstn_arr.shape)
    y_size = pstn_arr.shape[0]
    x_size = pstn_arr.shape[1]

    lut = [[0] * x_size for j in range(y_size)]

    f = open('lut_file1.csv', 'w', encoding='utf-8-sig', newline="")
    csv_write = csv.writer(f)
    csv_write.writerow(['r', 'c', 'i', 'j', 'Xp', 'Yp', 'Zp', 'lon', 'lat', 'alt'])

    for y in range(y_size):
        for x in range(x_size):
            Xp = pstn_arr[y, x, 0]
            Yp = pstn_arr[y, x, 1]
            Zp = pstn_arr[y, x, 2]

            t = symbols('t')
            R = symbols('R')
            Xs = orbital_paras[0][0] * t ** 3 + orbital_paras[0][1] * t ** 2 + orbital_paras[0][2] * t + \
                 orbital_paras[0][3]
            Ys = orbital_paras[1][0] * t ** 3 + orbital_paras[1][1] * t ** 2 + orbital_paras[1][2] * t + \
                 orbital_paras[1][3]
            Zs = orbital_paras[2][0] * t ** 3 + orbital_paras[2][1] * t ** 2 + orbital_paras[2][2] * t + \
                 orbital_paras[2][3]
            Vsx = orbital_paras[3][0] * t ** 3 + orbital_paras[3][1] * t ** 2 + orbital_paras[3][2] * t + \
                  orbital_paras[3][3]
            Vsy = orbital_paras[4][0] * t ** 3 + orbital_paras[4][1] * t ** 2 + orbital_paras[4][2] * t + \
                  orbital_paras[4][3]
            Vsz = orbital_paras[5][0] * t ** 3 + orbital_paras[5][1] * t ** 2 + orbital_paras[5][2] * t + \
                  orbital_paras[5][3]
            # 构建约束方程，求解t,R
            # 求解方法：牛顿迭代法等
            # R = math.sqrt((Xs - Xp) ** 2 + (Ys - Yp) ** 2 + (Zs - Zp) ** 2)
            TSR = 2 * R / LightSpeed
            T = TSR - Td
            # fd = np.matmul(np.array([T ** 0, T ** 1, T ** 2, T ** 3, T ** 4]), doppler_d)
            fd = doppler_d[0] * T ** 0 + doppler_d[1] * T ** 1 + doppler_d[2] * T ** 2 + doppler_d[3] * T ** 3 + \
                 doppler_d[4] * T ** 4
            fd = 0
            # define objective function
            f1 = (Xs - Xp) ** 2 + (Ys - Yp) ** 2 + (Zs - Zp) ** 2 - R ** 2
            f2 = fd / 2 + (Vsx * (Xs - Xp) + Vsy * (Ys - Yp) + Vsz * (Zs - Zp)) / (λ * R)
            f21 = (Vsx * (Xs - Xp) + Vsy * (Ys - Yp) + Vsz * (Zs - Zp)) / (λ * R)
            # f22 = fd / 2
            print("Before Differentiation:{}".format(f1))
            print("Before Differentiation:{}".format(f21))
            # take derivative of t and R
            grad_11 = diff(f1, t)
            print("After Differentiation:{}".format(grad_11))
            grad_12 = diff(f1, R)
            print("After Differentiation:{}".format(grad_12))
            grad_21 = diff(f21, t)
            print("After Differentiation:{}".format(grad_21))
            grad_22 = diff(f21, R)
            print("After Differentiation:{}".format(grad_22))
            # print("After Differentiation:{}".format(grad_11))
            # print("After Differentiation:{}".format(grad_12))
            # print("After Differentiation:{}".format(grad_21))
            # print("After Differentiation:{}".format(grad_22))
            # define parameters
            iter_cnt = 0
            MaxIter = 1000
            epsilon = 0.1
            # define initial point
            t_value = 0
            R_value = 1000000

            current_f1 = (float)(f1.subs({t: t_value, R: R_value}).evalf())
            current_f2 = (float)(f2.subs({t: t_value, R: R_value}).evalf())
            grad_11_value = (float)(grad_11.subs({t: t_value, R: R_value}).evalf())
            grad_12_value = (float)(grad_12.subs({t: t_value, R: R_value}).evalf())
            grad_21_value = (float)(grad_21.subs({t: t_value, R: R_value}).evalf())
            grad_22_value = (float)(grad_22.subs({t: t_value, R: R_value}).evalf())

            A = np.array([
                [grad_11_value, grad_12_value],
                [grad_21_value, grad_22_value]])

            X = np.array([
                [t_value],
                [R_value]])

            F = np.array([
                [current_f1],
                [current_f2]])

            while (abs(current_f1) + abs(current_f2)) >= epsilon and iter_cnt <= MaxIter:
                iter_cnt += 1
                print(abs(current_f1) + abs(current_f2))
                # 矩阵的转置
                X = X - np.dot(np.linalg.inv(A), F)
                t_value = X[0, 0]
                R_value = X[1, 0]
                grad_11_value = (float)(grad_11.subs({t: t_value, R: R_value}).evalf())
                grad_12_value = (float)(grad_12.subs({t: t_value, R: R_value}).evalf())
                grad_21_value = (float)(grad_21.subs({t: t_value, R: R_value}).evalf())
                grad_22_value = (float)(grad_22.subs({t: t_value, R: R_value}).evalf())

                A = np.array([
                    [grad_11_value, grad_12_value],
                    [grad_21_value, grad_22_value]])

                current_f1 = (float)(f1.subs({t: t_value, R: R_value}).evalf())
                current_f2 = (float)(f2.subs({t: t_value, R: R_value}).evalf())

                F = np.array([
                    [current_f1],
                    [current_f2]])

            print(abs(current_f1) + abs(current_f2))
            print("迭代次数：", iter_cnt)
            t_value = X[0, 0] + T0
            R_value = X[1, 0]

            # 得到像素的行列号
            r = (t_value - t0) / delta_t
            c = (R_value - R0) / delta_R

            lut[y][x] = [r, c, y, x, Xp, Yp, Zp]
            print(lut[y][x])
            [lon, lat, alt] = OrthoAuxData.XYZ2LLA(Xp, Yp, Zp)
            csv_write.writerow([r, c, y, x, Xp, Yp, Zp, lon, lat, alt])

    f.close()
    pass